import edu.wsu.KheperaSimulator.RobotController;

/**
 * 
 * The Controller3c class. The robot pushes a ball and balances it. Bang-bang
 * controller
 * 
 * @author Philipp Rusch und Markus Wetzel
 */
public class Controller4c extends RobotController {
	private boolean ballFound = false;

	// Sensoren:
	// 0 .. Links
	// 1 .. Links Vorne Schraeg
	// 2 .. links Vorne
	// 3 .. Rechts Vorne
	// 4 .. Rechts Vorne Schraeg
	// 5 .. Rechts
	// 6 .. Rechts Hinten
	// 7 .. Links Hinten

	@Override
	public void close() throws Exception {
	}

	@Override
	public void doWork() throws Exception {
		int threshold = 120;
		int threshold2 = 800;

		setMotorSpeeds(5, 5);
		setWaitTime(5);
		// Ganz links
		if (getDistanceValue(0) > threshold) {
			setMotorSpeeds(-9, 9);
		}
		// Vorne halb links
		if (!ballFound && getDistanceValue(1) > threshold) {
			setMotorSpeeds(-7, 7);
		}
		// Vorne halb rechts
		if (!ballFound && getLightValue(4) < threshold) {
			setMotorSpeeds(7, -7);
		}
		// Ganz rechts
		if (getDistanceValue(5) > threshold) {
			setMotorSpeeds(9, -9);
		}

		if (getDistanceValue(3) > threshold2
				&& getDistanceValue(4) > threshold2) {
			ballFound = true;
		}

		// if (getDistanceValue(4) > threshold2) {
		// setMotorSpeeds(6, 6);
		// }
	}

}
